#include <iostream>
#include <librealsense2/rs.hpp>
#include <iomanip>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>

using namespace std;
using namespace cv;

int main (int argc,char **argv)
{
    rs2::pipeline pipe;
    rs2::config cfg;
    cfg.enable_stream(RS2);
    cfg.enable_stream(RS2_STREAM_FISHEYE,1,RS2_FORMAT_Y8);
    
    pipe.start(cfg);
    rs2::pipeline_profile pipeline_profile=pipe.get_active_profile();
    rs2::stream_profile camera_profile=pipeline_profile.get_stream(RS2_STREAM_FISHEYE,1);
    
    rs2_intrinsics refofcamera=camera_profile.as<rs2::video_stream_profile>().get_intrinsics();
    Mat coeffin(1,4,CV_64F);
    for(int i=0;i<4;i++)
    {
        coeffin.at<double>(0,i)=refofcamera.coeffs[i];
    }
    
    Mat matofcamera=Mat::zeros(3,3,CV_64F);
    matofcamera.at<double>(0,0)=refofcamera.fx;
    matofcamera.at<double>(0,2)=refofcamera.ppx;
    matofcamera.at<double>(1,1)=refofcamera.fy;
    matofcamera.at<double>(1,2)=refofcamera.ppy;
    matofcamera.at<double>(2,2)=1;
    
    int count;
    
    while(true)
    {
        auto frames=pipe.wait_for_frames();
        
        rs2::video_frame image_frame=frames.get_fisheye_frame(1);
        
        Mat rawpicture(Size(848,800),CV_8U,(void*)image_frame.get_data(),Mat::AUTO_STEP);
        if(rawpicture.empty())
        {
            cout<<"can't get picture"<<endl;
            return -1;
        }
        namedWindow("the raw picture",WINDOW_AUTOSIZE);
        imshow("the raw picture",rawpicture);
        waitKey(5);
    }
    return 0;
}
